//
// Created by bit on 2022/8/3.
//

#include "guidance_filter/BlosFilter.h"

bool BlosFilter::update(float vangle, float hangle, LOS_STATE &state) {
    bool ret{false};
    switch (init_phase_) {
        case 0: {
            std::cout << "[BLOS FILTER]init state: vertical "
            << vangle << ", horizontal " << hangle << std::endl;
            v_init_ = vangle;
            h_init_ = hangle;
            setInitState(0.0f, 0.0f, 0.0f, 0.0f);
            init_phase_ += 1;
            break;
        }
        default: {
            ret = true;

            if (time_loop_) {
                dt_ = myelapsedTime(time_loop_) / 1000.0f;
            }
            time_loop_ = mycurrentTime();
            vangle -= v_init_;
            hangle -= h_init_;
            hangle = angle_lim(hangle, 180);

//            float y;
//            std::ostringstream os;
////            std::cout << "save func log to " << log_index<< std::endl;
//            os << "log/func_log" << log_index++ << ".csv";
//            blos_data_.open(os.str().c_str());
//            for (int i = -1000; i < 1000; i ++) {
//                float x2 = i/10.0f;
//                y = diff_func(x2, dt_, hangle, los_previous_.horizontal, los_previous_.rate_hor);
//                blos_data_ << x2 << "," << y << std::endl;
//            }
//            blos_data_.close();

            state.rate_hor = dichotomy(-100.0f, 100.0f, dt_, hangle,
                                       los_previous_.horizontal, los_previous_.rate_hor);
            state.rate_ver = dichotomy(-100.0f, 100.0f, dt_, vangle,
                                       los_previous_.vertical, los_previous_.rate_ver);

            state.horizontal = los_previous_.horizontal + dt_ * state.rate_hor;
            state.vertical = los_previous_.vertical + dt_ * state.rate_ver;
            los_previous_ = state;
            state.vertical += v_init_;
            state.horizontal += h_init_;
            state.horizontal = angle_lim(state.horizontal, 180);

//            std::cout << "horizon rate " << state.rate_hor << ", vertical " << state.rate_ver << std::endl;
//            output_data_ << vangle << "," << hangle << "," << state.vertical
//                         << "," << state.horizontal << "," << state.rate_ver << "," << state.rate_hor
//                         << "," << dt_ << std::endl;
        }
            break;
    }

    return ret;
}

void BlosFilter::setInitState(float ver, float hor, float rate_ver, float rate_hor) {
    los_previous_.vertical = ver;
    los_previous_.horizontal = hor;
    los_previous_.rate_hor = rate_hor;
    los_previous_.rate_ver = rate_ver;
}

float BlosFilter::diff_func(float x2, float dt, float blos, float x1_previous, float x2_previoius) {
    float ret = (x2-x2_previoius) / (pow(R_, 2) * dt) + a0_ * (x1_previous + x2*dt - blos)
            + x2 * b0_/R_ + b1_* pow(fabsf(x2/R_), alpha_) * getSign(x2)
            + a1_ * pow(fabsf(x1_previous + x2*dt - blos), alpha_) * getSign(x1_previous + x2*dt - blos);
    return ret;
}

int BlosFilter::getSign(float input) {
    if (abs(input) < 1e-6)
        return 0;
    if (input > 0.0f)
        return 1;
    return -1;
}

float BlosFilter::dichotomy(float min, float max, float dt, float blos, float x1_previous, float x2_previoius) {
    float ret_min = diff_func(min, dt, blos, x1_previous, x2_previoius);
    float ret_max = diff_func(max, dt, blos, x1_previous, x2_previoius);

    if (ret_min * ret_max > 0) {
        float ret = (blos - x1_previous) / dt;
        std::cout << "can't get solve in this area, return x2 = " << ret << std::endl;
        return ret;
    }

    float middle = (min+max)/2;
    float retmiddle = diff_func(middle, dt, blos, x1_previous, x2_previoius);

    while (fabsf(retmiddle) > 0.001f) {
        (retmiddle * ret_min > 0.0f) ? min = middle : max = middle;
        middle = (min + max)/2;
        ret_min = diff_func(min, dt, blos, x1_previous, x2_previoius);
        ret_max = diff_func(max, dt, blos, x1_previous, x2_previoius);
        retmiddle = diff_func(middle, dt, blos, x1_previous, x2_previoius);
    }

    return middle;
}

// void BlosFilter::BLOS2LOS(ObjectLOS BLOS, float &LOS_vertical, float &LOS_horizon,
//                           const px4_msgs::msg::VehicleLocalPosition &uav_pos) {
// // 求目标在机体坐标系下的坐标
//     // 坐标系1，以飞机为原点的NED坐标系，LOS
//     // 坐标系2，相机坐标系
//     // 坐标系w，世界坐标系

//     Eigen::Quaterniond Q1 (1, 0, 0, 0);
// //    Eigen::Quaterniond Q2 (1, 0, 0, 0);
// //    Eigen::Quaterniond Q2 (-0.6284, -0.2505, 0.2229, 0.7019);   // west
// //    Eigen::Quaterniond Q2 (-0.9422, -0.3344, -0.0202, -0.0077); // north
// //    Eigen::Quaterniond Q2 (-0.707, 0, 0, -0.707); // east
// //    Eigen::Quaterniond Q2 (0.0110, 0.0334, -0.3283, -0.9439);   // south

//     Eigen::Quaterniond Q2 (BLOS.q1, BLOS.q2, BLOS.q3, BLOS.q4);

//     Q1.normalize();
//     Q2.normalize();

//     Eigen::Vector3d t1 (0, 0, 0);
//     Eigen::Vector3d p1; // 点在1下的坐标

// //    Eigen::Isometry3d w_body = Eigen::Isometry3d::Identity();   // 机体质心NED坐标系到机体坐标系的转换
// //    w_body.rotate(Q2);
// //    w_body.pretranslate(t1);
// //    Eigen::Vector3d t2 (0.406, 0, 0);
// //    t2 = w_body * t2;
//     Eigen::Vector3d t2 (0, 0, 0);
//     std::cout << "camera_pos x " << t2.x() << ", y " << t2.y() << ", z " << t2.z() << std::endl;

// //    Eigen::Vector3d p2 (100, 0, 0);
//     Eigen::Vector3d p2 (cos(BLOS.hangle / 57.3) * cos(BLOS.vangle / 57.3), cos(BLOS.vangle / 57.3) * sin(BLOS.hangle / 57.3),
//                         -sin(BLOS.vangle / 57.3));                            // 点在2下的坐标
//     Eigen::Vector3d pw;                            // 点在世界坐标系下的坐标

//     Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
//     Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();

//     T_1w.rotate(Q1);
//     T_1w.pretranslate(t1);
//     T_2w.rotate(Q2);
//     T_2w.pretranslate(t2);
//     //求解P2即点在2下的坐标
//     pw = T_2w * p2;
//     p1 = T_1w.inverse() * pw;
//     pw.normalize();
//     p1.normalize();
//     std::cout << "该点在世界坐标系下坐标为" << pw.transpose() << std::endl;
//     std::cout << "该点在LOS下的坐标为：" << p1.transpose() << std::endl;
//     std::cout << "该点在BLOS下的坐标为：" << p2.transpose() << std::endl;
//     std::cout << "pw: x " << p1.x() << ", y " << p1.y() << ", z " << p1.z() << std::endl;

//     LOS_vertical = asinf(-p1.z());
//     LOS_horizon = atan2f(p1.y(), p1.x());

//     LOS_vertical *= 57.3;
//     LOS_horizon *= 57.3;

// //    std::cout << "output LOS vangle " << vangle << ", hangle " << hangle <<  std::endl;

//     if (std::isnan(LOS_vertical) || std::isnan(LOS_horizon)) {
//         uint8_t test_num{0};
//         std::cout << "BLOS2LOS: wtf! " << test_num ++ << std::endl;
//     }

//     float gain = -uav_pos.z / pw.z();

//     output_data_ << pw.x() << "," << pw.y() << "," << pw.z() << ','
//     << pw.x() * gain + uav_pos.x << "," << pw.y() * gain + uav_pos.y << "," << pw.z() * gain + uav_pos.z << ','
//     << BLOS.q1 << "," << BLOS.q2 << "," << BLOS.q3 << "," << BLOS.q4 << ",";
// }

// void BlosFilter::BLOS_sim(const px4_msgs::msg::VehicleLocalPosition &vehicle, const px4_msgs::msg::VehicleAttitude &att,
//                           float N_target, float E_target, float D_target, float &vangle, float &hangle) {
//     // 求目标在机体坐标系下的坐标
//     // 坐标系1，以飞机为原点的NED坐标系
//     // 坐标系2，机体坐标系
//     // 坐标系w，世界坐标系
//     Eigen::Quaterniond Q1 (1, 0, 0, 0);
//     Eigen::Vector3d t1 (0, 0, 0);
//     Eigen::Vector3d t2 (0, 0, 0);
//     Eigen::Vector3d p1 (N_target-vehicle.x, E_target-vehicle.y, D_target-vehicle.z); // 点在1下的坐标
//     Eigen::Vector3d p2;                            // 点在2下的坐标
//     Eigen::Vector3d pw;                            // 点在世界坐标系下的坐标

//     Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
//     Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();

// //    std::cout << "receive q1 is " << att.q1 << ", receive q2 is " << att.q2 << ", receive q3 is " << att.q3 << std::endl;
//     Eigen::Quaterniond Q2 (att.q[0], att.q[1], att.q[2], att.q[3]);
//     Q1.normalize();
//     Q2.normalize();

//     T_1w.rotate(Q1);
//     T_1w.pretranslate(t1);
//     T_2w.rotate(Q2);
//     T_2w.pretranslate(t2);
//     //求解P2即点在2下的坐标
//     pw = T_1w * p1;
//     p2 = T_2w.inverse() * pw;
// //    std::cout << "该点在世界坐标系下坐标为" << pw.transpose() << std::endl;
// //    std::cout << "该点在二号下的坐标为：" << p2.transpose() << std::endl;

//     float dist_xy = pow(p2.transpose().x() * p2.transpose().x() + p2.transpose().y() * p2.transpose().y(), 0.5f);

//     if (dist_xy > 0.5f) {
//         vangle = atan2(-p2.transpose().z(), dist_xy) * 57.3f;
//         hangle = atan2(p2.transpose().y(), p2.transpose().x()) * 57.3f;
// //        std::cout << "V " << vangle << ", H " << hangle << std::endl;
//     } else {
//         hangle = 0.0f;
//         vangle = 0.0f;
//     }
//     output_data_ << vangle << "," << hangle << ",";
// }
